/*
 * @Description: 打印信息实现
 * @Author: Sang Hao
 * @Date: 2021-10-23 21:36:26
 * @LastEditTime: 2021-10-27 14:23:29
 * @LastEditors: Sang Hao
 */

#include <cmath>
#include <pcl/common/eigen.h>
#include "lidar_slam/tools/print_info.hpp"

namespace lidar_slam {
void PrintInfo::PrintPose(std::string head, Eigen::Matrix4f pose) {
    Eigen::Affine3f aff_pose;
    aff_pose.matrix() = pose;
    float x, y, z, roll, pitch, yaw;
    pcl::getTranslationAndEulerAngles(aff_pose, x, y, z, roll, pitch, yaw);
    std::cout << head
              << x << "," << y << "," << z << "," 
              << roll * 180 / M_PI << "," << pitch * 180 / M_PI << "," << yaw * 180 / M_PI
              << std::endl;
}
}